#include "cartesian_to_frenet.h"


// Function to calculate distance between two points
double CartesianToFrenet::distance(const Point& a, const Point& b) {
    return std::hypot(a.x - b.x, a.y - b.y);
}

// Function to find the projection of point p on the line segment a-b
Point CartesianToFrenet::getProjection(const Point& a, const Point& b, const Point& p) {
    Point projection;
    double length_square = distance(a, b) * distance(a, b);
    
    if (length_square < std::numeric_limits<double>::epsilon()) {
        // Line segment is too short
        return a;
    }
    
    double u = ((p.x - a.x) * (b.x - a.x) + (p.y - a.y) * (b.y - a.y)) / length_square;
    
    if (u < 0.0) {
        projection = a; // Closer to the first point
    } else if (u > 1.0) {
        projection = b; // Closer to the second point
    } else {
        projection.x = a.x + u * (b.x - a.x);
        projection.y = a.y + u * (b.y - a.y);
    }
    
    return projection;
}

// Main function to convert Cartesian coordinates to Frenet
void CartesianToFrenet::cartesianToFrenet(const std::vector<std::pair<double, double>>& xy_points, nav_msgs::Odometry& odom, double& s, double& d) {
    std::vector<Point> path(xy_points.size());
    for (size_t i = 0; i < xy_points.size(); ++i) {
        path[i].x = xy_points[i].first;
        path[i].y = xy_points[i].second;
    }

    Point p;
    p.x = odom.pose.pose.position.x;
    p.y = odom.pose.pose.position.y;

    double min_dist = std::numeric_limits<double>::max();
    Point closest_projection;
    size_t closest_segment_index = 0;

    // Iterate over each segment in the path to find the closest projection
    for (size_t i = 1; i < path.size(); ++i) {
        Point projection = getProjection(path[i-1], path[i], p);
        double dist = distance(p, projection);

        if (dist < min_dist) {
            min_dist = dist;
            closest_projection = projection;
            closest_segment_index = i;
        }
    }

    // Calculate the longitudinal coordinate 's' along the path
    s = 0.0;
    for (size_t j = 0; j < closest_segment_index - 1; ++j) {
        s += distance(path[j], path[j+1]);
    }
    s += distance(path[closest_segment_index - 1], closest_projection);

    // Calculate the lateral coordinate 'd' as the perpendicular distance from the path
    double dx = p.x - closest_projection.x;
    double dy = p.y - closest_projection.y;

    // Determine the sign of 'd' based on the cross product
    Point path_direction = { path[closest_segment_index].x - path[closest_segment_index - 1].x, path[closest_segment_index].y - path[closest_segment_index - 1].y };
    double cross_product = path_direction.x * dy - path_direction.y * dx;

    d = (cross_product >= 0) ? min_dist : -min_dist;
}
